import lejos.nxt.LCD;
import lejos.robotics.subsumption.Behavior;

/**
 * Trouve la ligne droite
 * @author Nicolas
 */
public class TrouverLigneADroite implements Behavior {

    /**
     *  Fait bifurquer le robot vers la droite
     * @return True quand le robot est dans le gris foncé+sens normal ou gris clair+sens inversé
     */
    public boolean takeControl() {
        try {
            //s'il est alors trop à gauche par rapport à la ligne=>on va le faire tourner vers la droite
            int lightValue = Robot.readLightValue();
            return (Robot.GRIS_FONCE.contient(lightValue) && Robot.estEnSensNormal()) || (Robot.GRIS_CLAIR.contient(lightValue) && !Robot.estEnSensNormal());

        } catch (Exception ex) {
            Robot.justSay("Erreur takeControl TrouverLigneADroite");
            return false;
        }
    }

    /**
     * action appelée après action()
     */
    public void suppress() {
        try {
        } catch (Exception ex) {
            Robot.justSay("Erreur suppress TrouverLigneADroite");
        }
    }

    /**
     * action effectuée
     */
    public void action() {
            LCD.drawString("Trouver droite !", 0, 0);
            LCD.refresh();
            Robot.effectuerBalayage();        
    }
    public void action_old() {
        try {
           // LCD.clear();
            LCD.drawString("Trouver droite !", 0, 0);
            LCD.refresh();
            Robot.effectuerBalayage();
            
            int pas=-2;
            float pas_steer=5f;
            int i=0;
            //Si le robot est en navigation inverse, on inverse l'angle
            if(!Robot.estEnSensNormal()) pas=0-pas;
            //Tant qu'on n'a pas trouvé la ligne, on tourne par pas        
            while(Robot.GRIS_FONCE.contient(Robot.readLightValue())){
                LCD.drawString("valeur"+i+" "+Robot.readLightValue(), 0, 1);
                i++;
                LCD.refresh();
                
				Robot.MoteurDroit.forward();
				Robot.MoteurGauche.backward();
				
				while(!Robot.estSurLaLigne()){
					if(Robot.estSurDuNoir()){
						Robot.MoteurDroit.backward();
						Robot.MoteurGauche.forward();                
					}
				}
				
                //arc vers la droite avec roue droite bloquée
                if(Robot.estEnSensNormal()){
                //    Robot.Pilot.steer(-100, pas_steer);
                } else {
                //    Robot.Pilot.steer(100, pas_steer);
                }
            }
        } catch (Exception ex) {
            Robot.justSay("Erreur action TrouverLigneADroite");
        }
    }
}
